clear;clc;
c1 = -3.117;c2 = -58.221;k1 = 1/165.827;k2 = 1/76.661;k3 = 1/210.827;
Raa = [0,c1*k1,0;c1*k2,0,c2*k2;0,c2*k3,0];
Rsa = [k1,0,0;0,k2,0;0,0,k3];
Rsa = inv(eye(3)-Raa)*Rsa;
Rxs = [+37.5, 0, -10.1, -37.8;
    0, -569.9, 0, -1047.5;
    0, -239.4, 0, -210.9];
Rfs = [0, 0, 1;
    -189.97, -379.943, 0;
    171, -228, 0];
Rxx = [0,1,0,0;0,0,0,0;0,0,0,0;0,0,0,0];
Rax = [0,0,0;0,1,0;1,0,0;0,0,1];
Rxx1 = Rax*Rsa*Rxs;
B = Rax*Rsa*Rfs;
A = Rxx + Rxx1;
